#!/usr/bin/env python3

import paho.mqtt.client as mqtt
from simple_main_logic import AGVController  # 引入主控制逻辑

class AGV:
    def __init__(self, broker_ip="172.20.10.4", port=1883):
    # def __init__(self, broker_ip="localhost", port=1883):
        self.broker_ip = broker_ip
        self.port = port
        self.topic_response = "/agv/response"

        self.client = mqtt.Client()
        self.client.on_connect = self.on_connect
        self.client.on_message = self.on_message
        self.client.connect(self.broker_ip, self.port, 60)

    def on_connect(self, client, userdata, flags, rc):
        print("AGV Connected to MQTT broker")
        self.client.subscribe(self.topic_response)

    def on_message(self, client, userdata, msg):
        if msg.topic == self.topic_response:
            command = msg.payload.decode()
            if command == "START":
                print("[AGV] Received START command. Launching main logic...\n")
                controller = AGVController()
                controller.initialize(goal=(5.0, 5.0))  # 固定目标点
                controller.run()

    def run(self):
        print("AGV waiting for command...")
        self.client.loop_forever()



if __name__ == "__main__":
    # agv = AGV()
    # agv.run()
    import rospy
    rospy.init_node('communication', anonymous=True)
    controller = AGVController()
    controller.initialize(goal=(5.0, 5.0))  # 固定目标点
    controller.run()